#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
#include "vehicle_lmpc_base.hpp"

namespace auto_ros
{
namespace control
{
class VehicleLL4d4sMpc : public VehicleLMpcBase
{
public:
	VehicleLL4d4sMpc(const std::string dyn_mpc_kf_config_path_name, const std::string vehicle_lmpc_yaml_name, double rate);
	virtual void init_mpc();
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					  const PlanningRefPoint &ref_point, double speed,
					  const std::map<std::string, double> other_input);
	virtual bool reset_kf();
	virtual bool reset_kf(const Eigen::VectorXd &state_esti);

private:
	void calc_ax_max(double ax, double ay, std::vector<double> &ax_max_vec);
	void update_friction_circle(double speed, double ax, double ay);
};

} // namespace control
} // namespace auto_ros